#pragma config(Sensor, dgtl1,  right,          sensorSONAR_inch)
#pragma config(Sensor, dgtl3,  left,           sensorSONAR_inch)
#pragma config(Sensor, dgtl8,  up,             sensorTouch)
#pragma config(Sensor, dgtl9,  down,           sensorTouch)
#pragma config(Sensor, dgtl10, button,         sensorTouch)
#pragma config(Motor,  port2,           m2,            tmotorServoContinuousRotation, openLoop)//r
#pragma config(Motor,  port3,           m3,            tmotorServoContinuousRotation, openLoop, reversed)//l
#pragma config(Motor,  port4,           m4,            tmotorServoContinuousRotation, openLoop, reversed)//1
#pragma config(Motor,  port5,           m5,            tmotorServoStandard, openLoop)//r
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

bool uphill = true;
bool flat = false;
int rstate;
int lstate;

task slope(){
  while (true) {
    if ((SensorValue(down) == 0) && (SensorValue(up) == 1)) {
      uphill=true;
      flat=false;
    }
    if ((SensorValue(up) == 0) && (SensorValue(down) == 1)) {
      uphill=false;
      flat=false;
    }
    if ((SensorValue(up) == 0) && (SensorValue(down) == 0)){
      flat=true;
    }
  }
}

task lsensorAvg(){
  int lpolls[10];
  while(true){
    int i = 0; int laccum = 0;
    while(i<10){
      lpolls[i] = SensorValue(left);
      wait1Msec(5);
      i++;
    }
    laccum = (lpolls[0]+lpolls[1]+lpolls[2]+lpolls[3]+lpolls[4]+lpolls[5]+lpolls[6]+lpolls[7]+lpolls[8]+lpolls[9]);
    laccum = laccum/10;
    lstate = laccum;
    for(int k = 0; k<10;k++)
      lpolls[k]= 0;
  }
}

task rsensorAvg(){
  int rpolls[10];
  while(true){
    int l = 0; int raccum = 0;
    while(l<10){
      rpolls[l] = SensorValue(right);
      wait1Msec(5);
      l++;
    }
    raccum = (rpolls[0]+rpolls[1]+rpolls[2]+rpolls[3]+rpolls[4]+rpolls[5]+rpolls[6]+rpolls[7]+rpolls[8]+rpolls[9]);
    raccum = raccum/10;
    rstate = raccum;
    for(int j = 0; j<10;j++)
      rpolls[j]= 0;
  }
}


void forward(){
  if (uphill && !flat){
    motor[m2]=38;
    motor[m3]=38;
    motor[m4]=38;
    motor[m5]=38;
  }
  if (!uphill && !flat) {
    motor[m2]=10;
    motor[m3]=10;
    motor[m4]=10;
    motor[m5]=10;
  }
  if (flat){
    motor[m2]=25;
    motor[m3]=25;
    motor[m4]=25;
    motor[m5]=25;
  }
}

void leftturn(){
  if (uphill && !flat){
    motor[m2]=35;
    motor[m3]=-35;
    motor[m4]=-35;
    motor[m5]=35;
  }
  if (!uphill && !flat) {
    motor[m2]=10;
    motor[m3]=-10;
    motor[m4]=-10;
    motor[m5]=10;
  }

  if (flat) {
    motor[m2]=20;
    motor[m3]=-20;
    motor[m4]=-20;
    motor[m5]=20;

  }

}
void evenleftturn(){
  if (uphill && !flat){
    motor[m2]=35;
    motor[m3]=0;
    motor[m4]=0;
    motor[m5]=35;
  }
  if (!uphill && !flat) {
    motor[m2]=10;
    motor[m3]=0;
    motor[m4]=0;
    motor[m5]=10;
  }

  if (flat) {
    motor[m2]=35;
    motor[m3]=0;
    motor[m4]=0;
    motor[m5]=35;

  }

}

void evenrightturn(){
  if (uphill && !flat){
    motor[m2]=0;
    motor[m3]=35;
    motor[m4]=35;
    motor[m5]=0;
  }
  if (!uphill && !flat) {
    motor[m2]=0;
    motor[m3]=10;
    motor[m4]=10;
    motor[m5]=0;
  }
  if (flat) {
    motor[m2]=0;
    motor[m3]=35;
    motor[m4]=35;
    motor[m5]=0;

  }

}

void rightturn(){
  if (uphill && !flat){
    motor[m2]=-35;
    motor[m3]=35;
    motor[m4]=35;
    motor[m5]=-35;
  }
  if (!uphill && !flat) {
    motor[m2]=-10;
    motor[m3]=10;
    motor[m4]=10;
    motor[m5]=-10;
  }
  if (flat) {
    motor[m2]=20;
    motor[m3]=-20;
    motor[m4]=-20;
    motor[m5]=20;

  }

}

void back(){
  if (!uphill){
    motor[m2]=-35;
    motor[m3]=-35;
    motor[m4]=-35;
    motor[m5]=-35;
  }
  if (uphill) {
    motor[m2]=-10;
    motor[m3]=-10;
    motor[m4]=-10;
    motor[m5]=-10;
  }
}

void stop(){
  motor[m2]=0;
  motor[m3]=0;
  motor[m4]=0;
  motor[m5]=0;
}

task tempmain(){//roof program
  StartTask(slope);
  StartTask(lsensorAvg);
  StartTask(rsensorAvg);
  while(true){
    wait1Msec(20000);
  }
  while (1==1) {
    if (SensorValue(button) == 1){
      stop();
      break;
      break;
    }
    else if(rstate >12|| rstate<0){
      stop();
      //wait1Msec(2000);
      //int raccum = sensorAvg(right);

      evenrightturn();
      wait1Msec(500);

    }
    else if(lstate>12 ||lstate<0) {
      stop();
      //wait1Msec(1000);
      //int laccum = sensorAvg(left);

      evenleftturn();
      wait1Msec(500);

    }

    else if ((lstate >12 ||lstate<0)&& (rstate >12||rstate<0)) {
      back();
      wait1Msec(500);
      stop();
      leftturn();
      wait1Msec(1000);
      stop();
    }

    else{
      forward();
    }


  }
}

task main(){//table program
  StartTask(slope);
  StartTask(lsensorAvg);
  StartTask(rsensorAvg);
  while(false){
    wait1Msec(20000);
  }
  while (1==1) {
    if (SensorValue(button) == 1){
      stop();
      break;
      //break;
    }
    else if((rstate >12|| rstate<0)&&(lstate<12 &&lstate>0)){ //right
      stop();
      //wait1Msec(2000);
      //int raccum = sensorAvg(right);

      evenrightturn();
      wait1Msec(100);

    }
    else if((lstate>12 ||lstate<0)&&(rstate <12&& rstate>0)) { //left
      stop();
      //wait1Msec(1000);
      //int laccum = sensorAvg(left);

      evenleftturn();
      wait1Msec(100);

    }

    else if ((lstate >12 ||lstate<0)&& (rstate >12||rstate<0)) {
      back();
      wait1Msec(500);
      stop();
      leftturn();
      wait1Msec(1000);
      stop();
    }

    else{
      forward();
    }


  }
}
